/*
 * Interpolation2DBase has the following inherited classes:
 * - PolInt2D for General Polynomial Interpolation for 2 dim
 * - LinInt2D for Efficient Linear Interpolation for 2 dim
 * - HerInt2D for Hermite Interpolation for 2 dim
 * - SplInt2D for Spline Interpolation for 2 dim
 * 
 * Author: Yuejia Zhang
 * Date: 2023/2/21
 * 
 * */

#ifndef INTERPOLATION_2D
#define INTERPOLATION_2D

#include <cmath>
#include <iostream>

template <class InternalType = double, class ExternalType = float>
class Interpolation2DBase {
 protected:
  int m_, n_;  // number of intervals, one less than number of knots
  ExternalType *xa_, *ya_;
  ExternalType **za_;
  InternalType **coefficients;

 public:
  /* The construct function computes and stores the coefficients
   * (or second derivatives). xa, ya, za are arrays of x, y, z,
   * where za is a 2d-array. m is the dimension of xa,
   * n is the dimension of ya. */
  virtual void construct(ExternalType xa[], ExternalType ya[],
                         ExternalType **za) = 0;

  /* The evaluate function requires to find the place in the table,
   * at worst O(log N). */
  virtual ExternalType evaluate(ExternalType x, ExternalType y) = 0;

  /* subfunction of evaluate, using binary search to locate the interval
   * (xa[i], xa[i+1]) where x is in. returns the left index i.
   * xa must be a sorted array in increasing order!
   * also handles the boundary condition, see the first two lines. */
  int binarySearch(ExternalType *xa, int n, ExternalType x) {
    if (x < xa[0]) return -1;
    if (x > xa[n - 1]) return n;
    int ib = 0, ie = n - 1;
    int im = (ib + ie) / 2;
    while (ib < im) {
      if (x < xa[im])
        ie = im;
      else
        ib = im;
      im = (ib + ie) / 2;
    }
    return im;
  }

  /* subfunction of evaluate, use linear interpolation when either
   * x < x_min or y < y_min.
   * the linear function is induced from grid
   * (xa[ib], xa[ib+1]) x (ya[jb], ya[jb+1])*/
  ExternalType evaluateLinear(ExternalType x, ExternalType y, int ib, int jb) {
    ExternalType *xa = this->xa_, *ya = this->ya_;
    ExternalType **za = this->za_;
    InternalType dx = xa[ib + 1] - xa[ib], dy = ya[jb + 1] - ya[jb],
                 xdx = x - xa[ib], ydy = y - ya[jb];
    InternalType c1, c2, c3, c4;
    c1 = za[ib][jb];
    c2 = (za[ib + 1][jb] - za[ib][jb]) / dx;
    c3 = (za[ib][jb + 1] - za[ib][jb]) / dy;
    c4 = ((za[ib + 1][jb + 1] - za[ib][jb + 1]) / dx - c2) / dy;
    return c1 + c2 * xdx + (c3 + c4 * xdx) * ydy;
  }

  /* help function for solely debug purpose. */
  virtual void printCoefficients() = 0;
};

/* PolInt2D: Polynomial Interpolation for 2 dim, supports arbitrary N
 * */
template <class InternalType = double, class ExternalType = float, int N = 1>
class PolInt2D : public Interpolation2DBase<InternalType, ExternalType> {
 private:
  ExternalType **xproduct_, **yproduct_;
  InternalType *tv_;

 public:
  /* Constructor. Mainly for applying for space.
   * For fixed m and n and N, the class is constructed only once. */
  PolInt2D(int m, int n) {
    // the number of intervals = the number of points - 1
    m--;
    n--;
    this->m_ = m;
    this->n_ = n;
    // initialize **coefficients
    this->coefficients = new InternalType *[m * n];
    for (int i = 0; i < m * n; i++)
      this->coefficients[i] = new InternalType[(N + 1) * (N + 1)];

    // The following space is used in evaluation
    int NN = N + 1;
    // step 1: compute xproduct \prod (x_i-x_j) and yproduct
    // xproduct[i][j] = (x_i - x_0)...(x_i - x_{j-1})
    xproduct_ = new ExternalType *[NN];
    for (int i = 0; i < NN; i++) xproduct_[i] = new ExternalType[i + 1];
    yproduct_ = new ExternalType *[NN];
    for (int i = 0; i < NN; i++) yproduct_[i] = new ExternalType[i + 1];
    // step 2: compute a_{pq} by row
    tv_ = new InternalType[NN];
  }

  /* After calling construct, the (N+1)*(N+1) coefficients
   * for each grid (m*n) are determined, and stored in
   * coefficients[gridIdx][coefIdx],
   * where, 0 <= gridIdx < m*n, 0 <= coefIdx < (N+1)*(N+1). */
  void construct(ExternalType xa[], ExternalType ya[], ExternalType **za) {
    int m = this->m_;
    int n = this->n_;
    // initialize **coefficients
    for (int i = 0; i < m * n; i++)
      for (int j = 0; j < (N + 1) * (N + 1); j++)
        this->coefficients[i][j] = 0.0;

    this->xa_ = xa;
    this->ya_ = ya;
    this->za_ = za;

    // the inner layer
    int ib = (N - 1) / 2;
    int ie = m - N / 2;
    int jb = (N - 1) / 2;
    int je = n - N / 2;
    for (int i = ib; i < ie; i++)
      for (int j = jb; j < je; j++)
        constructGrid(xa, ya, za, this->coefficients[i * n + j], i - ib,
                      j - jb);

    // the outer layer
    for (int i = ib - 1; i >= 0; i--)
      for (int j = jb; j < je; j++)
        this->coefficients[i * n + j] = this->coefficients[(i + 1) * n + j];
    for (int j = jb - 1; j >= 0; j--)
      for (int i = 0; i < ie; i++)
        this->coefficients[i * n + j] = this->coefficients[i * n + (j + 1)];
    for (int i = ie; i < m; i++)
      for (int j = 0; j < je; j++)
        this->coefficients[i * n + j] = this->coefficients[(i - 1) * n + j];
    for (int j = je; j < n; j++)
      for (int i = 0; i < m; i++)
        this->coefficients[i * n + j] = this->coefficients[i * n + (j - 1)];
  }

  /* First find out the place of (x, y) in grid using binary search,
   * then evaluate the polynomial. */
  ExternalType evaluate(ExternalType x, ExternalType y) {
    int ic = this->binarySearch(this->xa_, this->m_ + 1, x);
    int jc = this->binarySearch(this->ya_, this->n_ + 1, y);
    if (ic > this->m_ - 1) ic = this->m_ - 1;
    if (jc > this->n_ - 1) jc = this->n_ - 1;
    if (ic < 0 || jc < 0) {
      if (ic < 0) ic = 0;
      if (jc < 0) jc = 0;
      return this->evaluateLinear(x, y, ic, jc);
    }
    // if the point falls in outer layer
    if (ic == 0) ic += (N - 1) / 2;
    if (ic == this->m_ - 1) ic -= N / 2;
    if (jc == 0) jc += (N - 1) / 2;
    if (jc == this->n_ - 1) jc -= N / 2;
    InternalType *cof = this->coefficients[ic * this->n_ + jc];
    int ib = ic - (N - 1) / 2;
    int jb = jc - (N - 1) / 2;
    ExternalType sum = 0, row_sum;
    for (int i = N; i >= 0; i--) {
      row_sum = 0;
      for (int j = N; j >= 0; j--)
        row_sum = row_sum * (y - this->ya_[jb + j]) + cof[i * (N + 1) + j];
      sum = sum * (x - this->xa_[ib + i]) + row_sum;
    }
    return sum;
  }

  /* subfunction of construct, compute the coefficients of grid (i, j)
   * and store the array in coefficients[i*n+j].
   * xa[ib:ib+N] and ya[jb:jb+N] are available data to use. */
  void constructGrid(ExternalType xa[], ExternalType ya[], ExternalType **za,
                     InternalType *ca, int ib, int jb) {
    int NN = N + 1;
    // step 1: compute xproduct \prod (x_i-x_j) and yproduct
    // xproduct[i][j] = (x_i - x_0)...(x_i - x_{j-1})
    ExternalType **xproduct = xproduct_;
    for (int i = 0; i < NN; i++) {
      xproduct[i][0] = 1;
      for (int j = 1; j < i + 1; j++)
        xproduct[i][j] = xproduct[i][j - 1] * (xa[ib + i] - xa[ib + j - 1]);
    }
    ExternalType **yproduct = yproduct_;
    for (int i = 0; i < NN; i++) {
      yproduct[i][0] = 1;
      for (int j = 1; j < i + 1; j++)
        yproduct[i][j] = yproduct[i][j - 1] * (ya[jb + i] - ya[jb + j - 1]);
    }

    // step 2: compute a_{pq} by row
    InternalType *tv = tv_;
    for (int p = 0; p < NN; p++)
      for (int q = 0; q < NN; q++) {
        InternalType res;
        res = za[ib + p][jb + q];
        for (int k = 0; k < q; k++) res -= tv[k] * yproduct[q][k];
        res /= yproduct[q][q];
        tv[q] = res;
        for (int k = 0; k < p; k++) res -= ca[k * NN + q] * xproduct[p][k];
        res /= xproduct[p][p];
        ca[p * NN + q] = res;
      }
  }

  void printCoefficients() {
    printf("===Debug: Print Coefficients===\n");
    int m = this->m_, n = this->n_;
    for (int i = 0; i < m; i++)
      for (int j = 0; j < n; j++) {
        printf("Grid (%d, %d) [%.1f, %.1f]x[%.1f, %.1f]: ", i, j, this->xa_[i],
               this->xa_[i + 1], this->ya_[j], this->ya_[j + 1]);
        for (int k1 = 0; k1 < N + 1; k1++)
          for (int k2 = 0; k2 < N + 1; k2++)
            printf("a_{%d%d} = %.2f; ", k1, k2,
                   this->coefficients[i * n + j][k1 * (N + 1) + k2]);
        printf("\n");
      }
    printf("================================\n");
  }

  ~PolInt2D() {
    // only delete[] the inner layer because the outer layer
    // points to the same array
    int ib = (N - 1) / 2;
    int ie = this->m_ - N / 2;
    int jb = (N - 1) / 2;
    int je = this->n_ - N / 2;
    for (int i = ib; i < ie; i++)
      for (int j = jb; j < je; j++)
        delete[] this->coefficients[i * this->n_ + j];
    delete[] this->coefficients;
    int NN = N + 1;
    for (int i = 0; i < NN; i++) delete[] xproduct_[i];
    delete[] xproduct_;
    for (int i = 0; i < NN; i++) delete[] yproduct_[i];
    delete[] yproduct_;
    delete[] tv_;
  }
};

/* LinInt2D: Efficient Linear Interpolation for 2 dim
 * */
template <class InternalType = double, class ExternalType = float>
class LinInt2D : public Interpolation2DBase<InternalType, ExternalType> {
 private:
  InternalType *coefficients;

 public:
  /* Constructor. Mainly for applying for space.
   * For fixed m and n, the class is constructed only once. */
  LinInt2D(int m, int n) {
    // the number of intervals = the number of points - 1
    m--;
    n--;
    // initialize *coefficients
    this->coefficients = new InternalType[4 * m * n];
    this->m_ = m;
    this->n_ = n;
  }

  /* The linear form: z = c1 + c2*x + c3*y + c4*x*y
   * After calling construct, the 4 coefficients for each grid (m*n)
   * are determined, and stored in coefficients[idx],
   * where 0 <= idx < 4*m*n. */
  void construct(ExternalType xa[], ExternalType ya[], ExternalType **za) {
    int m = this->m_;
    int n = this->n_;
    this->xa_ = xa;
    this->ya_ = ya;
    this->za_ = za;
    for (int i = 0; i < m; i++)
      for (int j = 0; j < n; j++)
        constructGrid(xa, ya, za, this->coefficients + 4 * (i * n + j), i, j);
  }

  /* The linear form: z = c1 + c2*x + (c3 + c4*x)*y
   * First find out the place of (x, y) in grid using binary search,
   * then evaluate the polynomial. */
  ExternalType evaluate(ExternalType x, ExternalType y) {
    int ic = this->binarySearch(this->xa_, this->m_ + 1, x);
    int jc = this->binarySearch(this->ya_, this->n_ + 1, y);
    if (ic > this->m_ - 1) ic = this->m_ - 1;
    if (jc > this->n_ - 1) jc = this->n_ - 1;
    if (ic < 0 || jc < 0) {
      if (ic < 0) ic = 0;
      if (jc < 0) jc = 0;
      return this->evaluateLinear(x, y, ic, jc);
    }
    InternalType *cof = this->coefficients + 4 * (ic * this->n_ + jc);
    return cof[0] + cof[1] * x + (cof[2] + cof[3] * x) * y;
  }

  /* The linear form: z = c1 + c2*x + (c3 + c4*x)*y
   * subfunction of construct, compute the coefficients of grid (i, j)
   * and store the array in coefficients[4(i*n+j):4(i*n+j)+3].*/
  void constructGrid(ExternalType xa[], ExternalType ya[], ExternalType **za,
                     InternalType *ca, int ib, int jb) {
    InternalType dx = xa[ib + 1] - xa[ib], dy = ya[jb + 1] - ya[jb];
    InternalType c1, c2, c3, c4;
    c1 = za[ib][jb];
    c2 = (za[ib + 1][jb] - za[ib][jb]) / dx;
    c3 = (za[ib][jb + 1] - za[ib][jb]) / dy;
    c4 = ((za[ib + 1][jb + 1] - za[ib][jb + 1]) / dx - c2) / dy;
    ca[0] = c1 - c2 * xa[ib] - (c3 - c4 * xa[ib]) * ya[jb];
    ca[1] = c2 - c4 * ya[jb];
    ca[2] = c3 - c4 * xa[ib];
    ca[3] = c4;
  }

  void printCoefficients() {
    printf("===Debug: Print Coefficients===\n");
    int m = this->m_, n = this->n_;
    InternalType *p = this->coefficients;
    for (int i = 0; i < m; i++)
      for (int j = 0; j < n; j++) {
        printf("Grid (%d, %d) [%.1f, %.1f]x[%.1f, %.1f]: ", i, j, this->xa_[i],
               this->xa_[i + 1], this->ya_[j], this->ya_[j + 1]);
        printf("c_1 = %.2f, c_2 = %.2f, c_3 = %.2f, c_4 = %.2f\n", p[0], p[1],
               p[2], p[3]);
        p += 4;
      }
    printf("================================\n");
  }

  ~LinInt2D() { delete[] this->coefficients; }
};

/* QuaInt2D: Quadratic Interpolation for 2 dim
 * */
template <class InternalType = double, class ExternalType = float>
class QuaInt2D : public Interpolation2DBase<InternalType, ExternalType> {
 private:
  InternalType *coefficients;

 public:
  /* Constructor. Mainly for applying for space.
   * For fixed m and n, the class is constructed only once. */
  QuaInt2D(int m, int n) {
    // the number of intervals = the number of points - 1
    m--;
    n--;
    this->m_ = m;
    this->n_ = n;
    // initialize *coefficients
    this->coefficients = new InternalType[9 * m * n];
  }

  /* The quadratic form: z = \sum_{i,j} c_{ij} x^i * y^j.  i = 0, 1, 2; j = 0,
   * 1, 2. In ternary, i.e. c_12 -> c[5] After calling construct, the 9
   * coefficients for each grid (m*n) are determined, and stored in
   * coefficients[idx], where, 0 <= idx < 9*m*n. */
  void construct(ExternalType xa[], ExternalType ya[], ExternalType **za) {
    int m = this->m_;
    int n = this->n_;
    // initialize **coefficients
    for (int i = 0; i < 9 * m * n; i++) this->coefficients[i] = 0.0;

    this->xa_ = xa;
    this->ya_ = ya;
    this->za_ = za;

    // the inner layer
    for (int i = 0; i < m - 1; i++)
      for (int j = 0; j < n - 1; j++)
        constructGrid(xa, ya, za, this->coefficients + 9 * (i * n + j), i, j);

    // the outer layer
    // i = m-1
    for (int j = 0; j < n; j++)
      for (int k = 0; k < 9; k++)
        this->coefficients[9 * ((m - 1) * n + j) + k] =
            this->coefficients[9 * ((m - 2) * n + j) + k];
    // j = n-1
    for (int i = 0; i < m; i++)
      for (int k = 0; k < 9; k++)
        this->coefficients[9 * (i * n + (n - 1)) + k] =
            this->coefficients[9 * (i * n + (n - 2)) + k];
  }

  /* First find out the place of (x, y) in grid using binary search,
   * then evaluate the polynomial. */
  ExternalType evaluate(ExternalType x, ExternalType y) {
    int ic = this->binarySearch(this->xa_, this->m_ + 1, x);
    int jc = this->binarySearch(this->ya_, this->n_ + 1, y);
    if (ic > this->m_ - 1) ic = this->m_ - 1;
    if (jc > this->n_ - 1) jc = this->n_ - 1;
    if (ic < 0 || jc < 0) {
      if (ic < 0) ic = 0;
      if (jc < 0) jc = 0;
      return this->evaluateLinear(x, y, ic, jc);
    }
    // if the point falls in outer layer
    if (ic == this->m_ - 1) ic--;
    if (jc == this->n_ - 1) jc--;
    InternalType *cof = this->coefficients + 9 * (ic * this->n_ + jc);
    ExternalType sum_x = 0, sum_y;
    for (int i = 2; i >= 0; i--) {
      sum_y = 0;
      for (int j = 2; j >= 0; j--)
        sum_y = sum_y * (y - this->ya_[jc + j]) + cof[i * 3 + j];
      sum_x = sum_x * (x - this->xa_[ic + i]) + sum_y;
    }
    return sum_x;
  }

  /* subfunction of construct, compute the coefficients of grid (i, j)
   * and store the array in coefficients[9(i*n+j):9(i*n+j)+8].
   * xa[ib:ib+2] and ya[jb:jb+2] are available data to use. */
  void constructGrid(ExternalType xa[], ExternalType ya[], ExternalType **za,
                     InternalType *ca, int ib, int jb) {
    // step 1: compute xproduct \prod (x_i-x_j) and yproduct
    ExternalType xproduct[9], yproduct[9];
    // xproduct[i * 3 + j] = (x_i - x_0)...(x_i - x_{j-1})
    for (int i = 0; i < 3; i++) {
      xproduct[i * 3] = 1;
      for (int j = 1; j < i + 1; j++)
        xproduct[i * 3 + j] =
            xproduct[i * 3 + j - 1] * (xa[ib + i] - xa[ib + j - 1]);
    }
    // yproduct[i * 3 + j] = (y_i - y_0)...(y_i - y_{j-1})
    for (int i = 0; i < 3; i++) {
      yproduct[i * 3] = 1;
      for (int j = 1; j < i + 1; j++)
        yproduct[i * 3 + j] =
            yproduct[i * 3 + j - 1] * (ya[jb + i] - ya[jb + j - 1]);
    }

    // step 2: compute a_{pq} by row
    // tv: temporary vector
    InternalType tv[3];
    for (int p = 0; p < 3; p++)
      for (int q = 0; q < 3; q++) {
        InternalType res;
        res = za[ib + p][jb + q];
        for (int k = 0; k < q; k++) res -= tv[k] * yproduct[q * 3 + k];
        res /= yproduct[q * 3 + q];
        tv[q] = res;
        for (int k = 0; k < p; k++) res -= ca[k * 3 + q] * xproduct[p * 3 + k];
        res /= xproduct[p * 3 + p];
        ca[p * 3 + q] = res;
      }
  }

  void printCoefficients() {
    printf("===Debug: Print Coefficients===\n");
    int m = this->m_, n = this->n_;
    for (int i = 0; i < m; i++)
      for (int j = 0; j < n; j++) {
        printf("Grid (%d, %d) [%.1f, %.1f]x[%.1f, %.1f]: ", i, j, this->xa_[i],
               this->xa_[i + 1], this->ya_[j], this->ya_[j + 1]);
        for (int k1 = 0; k1 < 3; k1++)
          for (int k2 = 0; k2 < 3; k2++)
            printf("a_{%d%d} = %.2f; ", k1, k2,
                   this->coefficients[9 * (i * n + j) + k1 * 3 + k2]);
        printf("\n");
      }
    printf("================================\n");
  }

  ~QuaInt2D() { delete[] this->coefficients; }
};

/* HerInt2D: BiCubic Hermite Interpolation for 2 dim
 * */
template <class InternalType = double, class ExternalType = float>
class HerInt2D : public Interpolation2DBase<InternalType, ExternalType> {
 private:
  ExternalType *xa_new_, *ya_new_;
  ExternalType **za_new_, **za_off_;

 public:
  /* Constructor. Mainly for applying for space.
   * For fixed m and n, the class is constructed only once. */
  HerInt2D(int m, int n) {
    // the number of intervals = the number of points - 1
    this->m_ = m - 1;
    this->n_ = n - 1;
    // initialize **coefficients
    this->coefficients = new InternalType *[this->m_ * this->n_];
    for (int i = 0; i < this->m_ * this->n_; i++)
      this->coefficients[i] = new InternalType[16];

    // add padding to facilitate gradient computation
    xa_new_ = new ExternalType[m + 2];
    ya_new_ = new ExternalType[n + 2];
    za_new_ = new ExternalType *[m + 2];
    for (int i = 0; i < m + 2; i++) za_new_[i] = new ExternalType[n + 2];
    za_off_ = new ExternalType *[m + 2];
  }

  /* After calling construct, the 16 coefficients
   * for each grid (m*n) are determined, and stored in
   * coefficients[gridIdx][coefIdx],
   * where, 0 <= gridIdx < m*n, 0 <= coefIdx < 16. */
  void construct(ExternalType xa[], ExternalType ya[], ExternalType **za) {
    int m = this->m_ + 1;
    int n = this->n_ + 1;
    this->xa_ = xa;
    this->ya_ = ya;
    this->za_ = za;

    // add padding to facilitate gradient computation
    ExternalType *xa_new = xa_new_;
    xa_new[0] = xa[0];
    for (int i = 1; i < m + 1; i++) xa_new[i] = xa[i - 1];
    xa_new[m + 1] = xa[m - 1];
    ExternalType *ya_new = ya_new_;
    ya_new[0] = ya[0];
    for (int j = 1; j < n + 1; j++) ya_new[j] = ya[j - 1];
    ya_new[n + 1] = ya[n - 1];
    ExternalType **za_new = za_new_;
    // (0, 0), (i, i-1), (m+1, m-1)
    for (int i = 0, ik = 0; i < m + 2; i++, ik++) {
      za_new[i][0] = za[ik][0];
      for (int j = 1; j < n + 1; j++) za_new[i][j] = za[ik][j - 1];
      za_new[i][n + 1] = za[ik][n - 1];
      if (i == 0 || i == m) ik--;
    }
    ExternalType **za_off = za_off_;
    for (int i = 0; i < m + 2; i++) za_off[i] = za_new[i] + 1;

    // call constructGrid for every grid, plus one so that
    // xa[-1], ya[-1] and za[-1][-1] are supported
    for (int i = 0; i < this->m_; i++)
      for (int j = 0; j < this->n_; j++)
        constructGrid(xa_new + 1, ya_new + 1, za_off + 1,
                      this->coefficients[i * this->n_ + j], i, j);
  }

  /* First find out the place of (x, y) in grid using binary search,
   * then evaluate the polynomial. */
  ExternalType evaluate(ExternalType x, ExternalType y) {
    int ib = this->binarySearch(this->xa_, this->m_ + 1, x);
    int jb = this->binarySearch(this->ya_, this->n_ + 1, y);
    if (ib > this->m_ - 1) ib = this->m_ - 1;
    if (jb > this->n_ - 1) jb = this->n_ - 1;
    if (ib < 0 || jb < 0) {
      if (ib < 0) ib = 0;
      if (jb < 0) jb = 0;
      return this->evaluateLinear(x, y, ib, jb);
    }
    InternalType *cof = this->coefficients[ib * this->n_ + jb];
    ExternalType x_new =
        (x - this->xa_[ib]) / (this->xa_[ib + 1] - this->xa_[ib]);
    ExternalType y_new =
        (y - this->ya_[jb]) / (this->ya_[jb + 1] - this->ya_[jb]);
    InternalType xproduct[4];
    xproduct[0] = 1;
    for (int i = 1; i < 4; i++) xproduct[i] = xproduct[i - 1] * x_new;
    ExternalType res = 0, row_sum, base = 1;
    ;
    for (int i = 0; i < 4; i++) {
      row_sum = 0;
      // Hermite: a_00, a_10, a_20, a_30, a_01, a_11, ...
      for (int j = 0; j < 4; j++) row_sum += cof[i * 4 + j] * xproduct[j];
      res += row_sum * base;
      base *= y_new;
    }
    return res;
  }

  /* subfunction of construct, compute the coefficients of grid (ic, jc)
   * and store the array in coefficients[ic*n+jc].
   * xa[ib:ib+N] and ya[jb:jb+N] are available data to use. */
  void constructGrid(ExternalType xa[], ExternalType ya[], ExternalType **za,
                     InternalType *ca, int ic, int jc) {
    static int wt[16][16] = {
        1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
        0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  -3, 3,  0,  0,
        -2, -1, 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  2,  -2, 0,  0,  1,  1,
        0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
        1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
        0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  -3, 3,  0,  0,
        -2, -1, 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  2,  -2, 0,  0,  1,  1,
        0,  0,  -3, 0,  3,  0,  0,  0,  0,  0,  -2, 0,  -1, 0,  0,  0,  0,  0,
        0,  0,  0,  0,  -3, 0,  3,  0,  0,  0,  0,  0,  -2, 0,  -1, 0,  9,  -9,
        -9, 9,  6,  3,  -6, -3, 6,  -6, 3,  -3, 4,  2,  2,  1,  -6, 6,  6,  -6,
        -3, -3, 3,  3,  -4, 4,  -2, 2,  -2, -2, -1, -1, 2,  0,  -2, 0,  0,  0,
        0,  0,  1,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  2,  0,  -2, 0,
        0,  0,  0,  0,  1,  0,  1,  0,  -6, 6,  6,  -6, -4, -2, 4,  2,  -3, 3,
        -3, 3,  -2, -1, -2, -1, 4,  -4, -4, 4,  2,  2,  -2, -2, 2,  -2, 2,  -2,
        1,  1,  1,  1};

    // step 1: construct b array
    InternalType d1 = xa[ic + 1] - xa[ic], d2 = ya[jc + 1] - ya[jc];
    InternalType b[16];
    for (int offset = 0, i = ic, j = jc; offset < 4;
         offset++, i = ic + offset % 2, j = jc + offset / 2) {
      b[offset] = za[i][j];
      b[offset + 4] =
          (za[i + 1][j] - za[i - 1][j]) / (xa[i + 1] - xa[i - 1]) * d1;
      b[offset + 8] =
          (za[i][j + 1] - za[i][j - 1]) / (ya[j + 1] - ya[j - 1]) * d2;
      b[offset + 12] = (za[i + 1][j + 1] - za[i + 1][j - 1] - za[i - 1][j + 1] +
                        za[i - 1][j - 1]) /
                       ((xa[i + 1] - xa[i - 1]) * (ya[j + 1] - ya[j - 1])) *
                       d1 * d2;
    }

    // step 2: matrix multiply
    for (int i = 0; i < 16; i++) {
      InternalType coef = 0.0;
      for (int j = 0; j < 16; j++) coef += wt[i][j] * b[j];
      ca[i] = coef;
    }
  }

  void printCoefficients() {
    printf("===Debug: Print Coefficients===\n");
    int m = this->m_, n = this->n_;
    for (int i = 0; i < m; i++)
      for (int j = 0; j < n; j++) {
        printf("Grid (%d, %d) [%.1f, %.1f]x[%.1f, %.1f]: ", i, j, this->xa_[i],
               this->xa_[i + 1], this->ya_[j], this->ya_[j + 1]);
        for (int k1 = 0; k1 < 16; k1++)
          printf("a_{%d} = %.2f; ", k1, this->coefficients[i * n + j][k1]);
        printf("\n");
      }
    printf("================================\n");
  }

  ~HerInt2D() {
    for (int i = 0; i < this->m_ * this->n_; i++)
      delete[] this->coefficients[i];
    delete[] this->coefficients;
    // free additional space
    for (int i = 0; i < this->m_ + 3; i++) delete[] za_new_[i];
    delete[] xa_new_, ya_new_, za_new_, za_off_;
  }
};

/* SplInt2D: BiCubic Spline Interpolation for 2 dim
 * */
template <class InternalType = double, class ExternalType = float>
class SplInt2D : public Interpolation2DBase<InternalType, ExternalType> {
 private:
  ExternalType *za1_;
  InternalType *ca_, *dy1_, *dy2_, *b_, *T_;

 public:
  /* Constructor. Mainly for applying for space.
   * For fixed m and n, the class is constructed only once. */
  SplInt2D(int m, int n) {
    this->m_ = m - 1;
    this->n_ = n - 1;
    // initialize **coefficients
    this->coefficients = new InternalType *[m];
    for (int i = 0; i < m; i++) this->coefficients[i] = new InternalType[n];
    // in evaluate
    za1_ = new ExternalType[m];
    ca_ = new InternalType[m];
    // in construct 1D
    // first-order & second-order difference
    dy1_ = new InternalType[n - 1];
    dy2_ = new InternalType[n - 2];
    b_ = new InternalType[n];
    T_ = new InternalType[n - 2];
  }

  /* After calling construct, the second-order derivatives M_i
   * for each row x are determined, and stored in
   * coefficients[rowIdx][i],
   * where, 0 <= rowIdx < m, 0 <= i < n. */
  void construct(ExternalType xa[], ExternalType ya[], ExternalType **za) {
    int m = this->m_ + 1;
    int n = this->n_ + 1;
    this->xa_ = xa;
    this->ya_ = ya;
    this->za_ = za;
    for (int i = 0; i < m; i++)
      construct1D(ya, za[i], this->coefficients[i], n);
  }

  /* First find out the place of (x, y) in grid using binary search,
   * then evaluate the polynomial. */
  ExternalType evaluate(ExternalType x, ExternalType y) {
    int ib = this->binarySearch(this->xa_, this->m_ + 1, x);
    int jb = this->binarySearch(this->ya_, this->n_ + 1, y);
    if (ib < 0 || jb < 0 || ib > this->m_ - 1 || jb > this->n_ - 1) {
      // std::cerr << "Spline Interpolation cannot handle extrapolation. "
      //     << "Switch to Linear Interpolation instead. " << std::endl;
      if (ib < 0) ib = 0;
      if (jb < 0) jb = 0;
      if (ib > this->m_ - 1) ib = this->m_ - 1;
      if (jb > this->n_ - 1) jb = this->n_ - 1;
      return this->evaluateLinear(x, y, ib, jb);
    }
    int m = this->m_ + 1;
    int n = this->n_ + 1;
    ExternalType *za = za1_;
    for (int i = 0; i < m; i++)
      za[i] =
          evaluate1D(this->ya_, this->za_[i], this->coefficients[i], n, y, jb);
    InternalType *ca = ca_;
    construct1D(this->xa_, za, ca, m);
    return evaluate1D(this->xa_, za, ca, m, x, ib);
  }

  /* Construct 1D spline, the arrays are of size n*/
  void construct1D(ExternalType ya[], ExternalType za[], InternalType *ca,
                   int n) {
    // first-order & second-order difference
    InternalType *dy1 = dy1_;
    InternalType *dy2 = dy2_;
    for (int i = 0; i < n - 1; i++) dy1[i] = ya[i + 1] - ya[i];
    for (int i = 0; i < n - 2; i++) dy2[i] = ya[i + 2] - ya[i];
    InternalType *b = b_;
    for (int i = 0; i < n - 1; i++) b[i] = (za[i + 1] - za[i]) / dy1[i];
    for (int i = 0; i < n - 2; i++) b[i] = 6 * (b[i + 1] - b[i]) / dy2[i];

    // solve Tx = b. T stores diagonal of T
    // TODO: Use LAPACK library to solve triangular systems?
    InternalType *T = T_;
    T[0] = 2;
    for (int i = 1; i < n - 2; i++) {
      InternalType scale = dy1[i] / dy2[i] / T[i - 1];
      T[i] = 2 - dy1[i] / dy2[i - 1] * scale;
      b[i] = b[i] - b[i - 1] * scale;
    }
    ca[0] = 0;
    ca[n - 1] = 0;
    if (n > 2) ca[n - 2] = b[n - 3] / T[n - 3];
    for (int i = n - 3; i > 0; i--)
      ca[i] = (b[i - 1] - dy1[i] / dy2[i - 1] * ca[i + 1]) / T[i - 1];
  }

  /* f = c0 + c1*x + c2*x*x + c3*x*x*x
   *   = c0 + x*(c1 + x*(c2 + x*c3))
   * */
  InternalType evaluate1D(ExternalType xa[], ExternalType za[],
                          InternalType *ca, int n, ExternalType x, int i) {
    x -= xa[i];
    InternalType c0, c1, c2, c3, dx = xa[i + 1] - xa[i];
    c0 = za[i];
    c1 = (za[i + 1] - za[i]) / dx - (ca[i + 1] / 6 + ca[i] / 3) * dx;
    c2 = ca[i] / 2;
    c3 = (ca[i + 1] - ca[i]) / dx / 6;
    return c0 + x * (c1 + x * (c2 + x * c3));
  }

  void printCoefficients() {
    printf("===Debug: Print Coefficients===\n");
    int m = this->m_ + 1, n = this->n_ + 1;
    printf("%d(%.1f)\t", 0, 0.0);
    for (int j = 0; j < n; j++) printf("%d(%.1f)\t", j, this->ya_[j]);
    for (int i = 0; i < m; i++) {
      printf("%d(%.1f)\t", i, this->xa_[i]);
      for (int j = 0; j < n; j++) printf("%.3f\t", this->coefficients[i][j]);
      printf("\n");
    }
    printf("================================\n");
  }

  ~SplInt2D() {
    for (int i = 0; i < this->m_ + 1; i++) delete[] this->coefficients[i];
    delete[] this->coefficients;
    delete[] za1_, ca_, dy1_, dy2_, b_, T_;
  }
};

#endif